
#include "System\F2806x_Device.h"
#include <stdio.h>
#include <string.h>

// use floating point math!
#define MATH_TYPE 1
#include "IQmathLib.h"
#include "Drivers\debugLED.h"
#include "Drivers\f2806xpwm_2xDC.h"
#include "Drivers\f2806xileg_vdc_2xDC.h"
#include "Drivers\QEPDriver.h"
#include "Drivers\QEPDriver2.h"
#include "dlog4ch.h"
#include "Timer.h"
#include "CANDriver.h"
#include "DataFile.h"
#include "MatCtrl\Controller.h"

extern "C" void InitSysCtrl(void);

// flash support
#define FLASH 1
extern "C" void InitFlash(void);
extern "C" void MemCopy(Uint16 *SourceAddr, Uint16* SourceEndAddr, Uint16* DestAddr);
extern Uint16 RamfuncsLoadStart, RamfuncsLoadEnd, RamfuncsRunStart;

#define CAN_BASE 0x020000

// Drivers
DebugLED ledDebug;

// Components
Timer timer;
Timer timerINT;
CANDriver canDrv;
QEPDriver qepDriver;
QEPDriver2 qepDriver2;
SDataFile df;

ControllerModelClass controller1; // mat controller, motor1
ControllerModelClass controller2; // mat controller, motor2

// main step time
//#define STEPTIME 0.00025 /* current ctrl tuning rate */
#define STEPTIME 0.001 /* standard steptime, 1ms */

// Interrupt
extern "C" interrupt void MainISR(void);

// DMC Drivers
// Instance a PWM driver instance
PWMGEN pwm1 = PWMGEN_DEFAULTS;
PWMGEN pwm2 = PWMGEN_DEFAULTS;

// Create an instance of DATALOG Module
DLOG_4CH dlog = DLOG_4CH_DEFAULTS;   
int16 DlogCh1 = 0;
int16 DlogCh2 = 0;
int16 DlogCh3 = 0;
int16 DlogCh4 = 0;

// Vars
Uint32 IsrTicker = 0;
Uint16 Rotation1 = 1;				// Motor 1 PWM direction
Uint16 Rotation2 = 1;				// Motor 2 PWM direction
float Upvdd = 0;

int main(void) 
{
	//=================================
	//	INITIALISATION - General
	//=================================
	InitSysCtrl();	// Device Life support & GPIO mux settings

	// init debug LED
	ledDebug.Init();
		
// Only used if running from FLASH
#ifdef FLASH		
// Copy time critical code and Flash setup code to RAM
// The  RamfuncsLoadStart, RamfuncsLoadEnd, and RamfuncsRunStart
// symbols are created by the linker. Refer to the linker files. 
	MemCopy(&RamfuncsLoadStart, &RamfuncsLoadEnd, &RamfuncsRunStart);

// Call Flash Initialization to setup flash waitstates
// This function must reside in RAM
	InitFlash();	// Call the flash wrapper init function
#endif //(FLASH)

	// Init Components
	timer.Init();
	timerINT.Init();
	canDrv.Init();
	qepDriver.Init();
	qepDriver2.Init();
	controller1.initialize();
	
	// Init Drivers + PINs
	pwm1.PeriodMax = 4000;  // 10kHz -> 80000000/4000 = 20000 /2 (up/down counter) -> 10000
	PWM_INIT_MACRO(pwm1)  
    pwm2.PeriodMax = 4000;  // 10kHz -> 80000000/4000 = 20000 /2 (up/down counter) -> 10000 
	PWM_INIT_MACRO(pwm2) 
	EALLOW;
	GpioCtrlRegs.GPAMUX1.bit.GPIO0 = 1;		// 0=GPIO, 1=EPWM1A, 2=Resv, 3=Resv
	GpioCtrlRegs.GPAMUX1.bit.GPIO1 = 1;		// 0=GPIO, 1=EPWM1B, 2=EMU (0), 3=COMP1OUT
	GpioCtrlRegs.GPAMUX1.bit.GPIO2 = 1;		// 0=GPIO, 1=EPWM2A, 2=Resv, 3=Resv
	GpioCtrlRegs.GPAMUX1.bit.GPIO3 = 1;		// 0=GPIO, 1=EPWM2B, 2=SPISOMIA, 3=COMP2OUT
	GpioCtrlRegs.GPBMUX1.bit.GPIO41 = 0;	// 0=GPIO,  1=EPWM7B,  2=Resv,  3=Resv
	GpioDataRegs.GPBCLEAR.bit.GPIO41 = 1;	// Set Low initially, keep in reset
	GpioCtrlRegs.GPBDIR.bit.GPIO41 = 1;		// 1=OUTput,  0=INput 
	EDIS;
	
	// Initialize DATALOG module
    dlog.iptr1 = &DlogCh1;
    dlog.iptr2 = &DlogCh2;
	dlog.iptr3 = &DlogCh3;
    dlog.iptr4 = &DlogCh4;
    dlog.trig_value = 0x1;
    dlog.size = 0x00c8;
    dlog.prescalar = 5;
    dlog.init(&dlog);

// Initialize ADC module
    ADC_MACRO_INIT()

	// Reset DataFile
	memset(&df, 0, sizeof(df));
	//df.M12_Reset = 1; // start in reset state
	df.M12_Reset = 0; // start in GO state
	
	// init and start PWM Int->MainISR
	EALLOW;	// This is needed to write to EALLOW protected registers
	PieVectTable.EPWM1_INT = &MainISR;
	EDIS;
// Enable PIE group 3 interrupt 1 for EPWM1_INT
    PieCtrlRegs.PIEIER3.bit.INTx1 = 1;
// Enable CNT_zero interrupt using EPWM1 Time-base
    EPwm1Regs.ETSEL.bit.INTEN = 1;   // Enable EPWM1INT generation 
    EPwm1Regs.ETSEL.bit.INTSEL = 1;  // Enable interrupt CNT_zero event
    EPwm1Regs.ETPS.bit.INTPRD = 1;   // Generate interrupt on the 1st event
	EPwm1Regs.ETCLR.bit.INT = 1;     // Enable more interrupts
	
// Enable CPU INT3 for EPWM1_INT:
	IER |= M_INT3;
// Enable global Interrupts and higher priority real-time debug events:
	EINT;   // Enable Global interrupt INTM
	ERTM;	// Enable Global realtime interrupt DBGM
	

//=================================
//	Endless LOOP
//=================================
	for(;;)  //infinite loop
	{
		timer.BeginPeriod();
		df.Counter++;
			
		/// enable/disable driver	
		if(df.M12_Reset)
		{
			GpioDataRegs.GPBCLEAR.bit.GPIO41 = 1;
		}
		else
		{
			GpioDataRegs.GPBSET.bit.GPIO41 = 1;
		}
		
		// send at 1000Hz/100 = 10Hz
		if( (df.Counter % 100) == 0)
		{
			canDrv.SendMBOX(10, (float)df.M1_StateOUT);
			canDrv.SendMBOX(11, df.M1_PositionIN);
			canDrv.SendMBOX(12, df.M1_OmegaIN);
			canDrv.SendMBOX(13, df.M1_IFdbkUIN*10);
			canDrv.SendMBOX(14, (float)df.M2_StateOUT);
			canDrv.SendMBOX(15, df.M2_PositionIN);
			canDrv.SendMBOX(16, df.M2_OmegaIN);
			canDrv.SendMBOX(17, df.M2_IFdbkUIN*10);
			
			canDrv.SendMBOX(26, df.LoopTimeMS);
			canDrv.SendMBOX(27, df.MissionTime);
			canDrv.SendMBOX(28, df.LoopTimeMSMAX);
			canDrv.SendMBOX(29, df.INTLoopTimeUS);
			canDrv.SendMBOX(30, df.INTLoopTimeUSMAX);	
		}
				
		// receive
		float ref1 = 0;
		if( canDrv.ReadCANMBOX(1, &ref1) )
		{
			df.M1_InRefIN = ref1;
		}
		float ref2 = 0;
		if( canDrv.ReadCANMBOX(3, &ref2) )
		{
			df.M2_InRefIN = ref2;
		}
		
		float mode1;
		if( canDrv.ReadCANMBOX(0, &mode1) )
		{
			df.M1_ModeIN = (Int32)mode1;
		}
		float mode2;
		if( canDrv.ReadCANMBOX(2, &mode2) )
		{
			df.M2_ModeIN = (Int32)mode2;
		}
		
		// ctrl tuning M2
/*		if( ((df.counter+2) % 10) == 0)
		{
			canDrv.SendMBOX(20, df.M2_DutyOUT, IsrTicker); // duty
		}
		if( ((df.counter+4) % 10) == 0)
		{
			canDrv.SendMBOX(22, df.M2_OmegaIN, IsrTicker); // speed
		}		
		else if( ((df.counter+6) % 10) == 0)
		{
			canDrv.SendMBOX(23, df.M2_PositionIN, IsrTicker); // position
		}
		else if( ((df.counter+8) % 10) == 0)
		{
			canDrv.SendMBOX(24, df.M2_InRefIN, IsrTicker); // ref
		}
		else
		{
			canDrv.SendMBOX(21, df.M2_IFdbkUIN, IsrTicker); // current
		}
*/
				
		// blink LED
		if( (df.Counter % 200) == 0)
		{
			ledDebug.Toggle(DebugLED::GREEN);
		}
				
		//-----------------------------------------------------------
		// finish loop
		df.LoopTimeMS = timer.ElapsedTime() * 1000; // get looptime (ms)
		if(df.LoopTimeMSMAX < df.LoopTimeMS) df.LoopTimeMSMAX = df.LoopTimeMS;

		while( !timer.HasElapsed(STEPTIME) )
		{
			// do something...
		};
		df.MissionTime += timer.ElapsedTime();
	}
}

// MainISR 
extern "C" interrupt void MainISR(void)
{	
	// Verifying the ISR
    IsrTicker++;
    timerINT.BeginPeriod();

	// calc QEP
	qepDriver.Calc();
	df.M1_OmegaIN = qepDriver.GetSpeed();
	df.M1_PositionIN = qepDriver.GetPosition();
	
	qepDriver2.Calc();
	df.M2_OmegaIN = qepDriver2.GetSpeed();
	df.M2_PositionIN = qepDriver2.GetPosition();
	
	// check if locked
	df.M1_LockedIN = qepDriver.IsLocked();
	df.M2_LockedIN = qepDriver2.IsLocked();
	//df.M1_LockedIN = 1; // fake locked state XXX
	//df.M2_LockedIN = 1; // fake locked state XXX
	
	// Vpdc voltage
	Upvdd = (float)AdcResult.ADCRESULT2;
 
	////////////////
	// MOTOR1 (PWM2)
	////////////////
	// ------------------------------------------------------------------------------
	//  Measure phase currents, subtract the offset and normalize from  (1.0pu: 10A, -1.0pu: -10A)
	// ------------------------------------------------------------------------------	
    df.M1_IFdbkUIN =-((float)AdcResult.ADCRESULT0-1487)/1241;
		
	// Run real controller
	controller1.Controller_U.Iscaled = df.M1_IFdbkUIN;
	controller1.Controller_U.OmegaRadS = df.M1_OmegaIN;
	controller1.Controller_U.PositionN = df.M1_PositionIN;
	controller1.Controller_U.Locked = df.M1_LockedIN;
	controller1.Controller_U.Mode = df.M1_ModeIN;
	controller1.Controller_U.InRef = df.M1_InRefIN;
	controller1.step();
	df.M1_EnabledOUT = controller1.Controller_Y.Enable;
	df.M1_StateOUT = controller1.Controller_Y.State;
	df.M1_DutyOUT = controller1.Controller_Y.DutyU;
	
	if( df.M1_StateOUT == 0 || df.M1_StateOUT == 10 ) df.M1_DutyOUT = 0; // safety kill (XXX:Fix in mat model)
	
	// limit Vref1 (if controller goes wacko), replace with 0.5 (12V at 24V PS) after testing!
	float VRef1 = df.M1_DutyOUT;
	if( VRef1 > 0.5 ) VRef1 = 0.5;
	else if( VRef1 < -0.5 ) VRef1 = -0.5;
	//-------------------------------------------------------------------------------
	//  Connect inputs of the PWM_DRV module and call the PWM signal generation macro
	// ------------------------------------------------------------------------------
    pwm2.MfuncC1 = (int16)(_IQabs(VRef1)*32768.0); // MfuncC1 is in Q15 (convert to -32768...32768 range!)
	PWM_MACRO(pwm2)							   	   // Calculate the new PWM compare values	
	if(VRef1 < 0)
	{
	  Rotation1 = 0;
	}
	else
	{
	  Rotation1 = 1;
	}
	   
	if(Rotation1==0)
 	{
  		EPwm2Regs.AQCSFRC.bit.CSFB = 0;		//Forcing Disabled on EPWM1B	  
  		EPwm2Regs.AQCSFRC.bit.CSFA = 1;		//EPWM2A forced low
  	}
	else if(Rotation1 == 1)
	{
  		EPwm2Regs.AQCSFRC.bit.CSFA = 0;		//Forcing Disabled on EPWM1A	  
  		EPwm2Regs.AQCSFRC.bit.CSFB = 1;		//EPWM2B forced low
  	}
	else
  	{
  		EPwm2Regs.AQCSFRC.bit.CSFA = 1;		//EPWM2A forced low
  		EPwm2Regs.AQCSFRC.bit.CSFB = 1;		//EPWM2B forced low	  
  	}	
	EPwm2Regs.CMPA.half.CMPA=pwm2.PWM1out; 
	

	////////////////
	// MOTOR2 (PWM1)
	////////////////
	// ------------------------------------------------------------------------------
	//  Measure phase currents, subtract the offset and normalize from  (1.0pu: 10A, -1.0pu: -10A)
	// ------------------------------------------------------------------------------
    df.M2_IFdbkUIN=-((float)AdcResult.ADCRESULT1-1449)/1241; 	
	
	// Run real controller
	controller2.Controller_U.Iscaled = df.M2_IFdbkUIN;
	controller2.Controller_U.OmegaRadS = df.M2_OmegaIN;
	controller2.Controller_U.PositionN = df.M2_PositionIN;
	controller2.Controller_U.Locked = df.M2_LockedIN;
	controller2.Controller_U.Mode = df.M2_ModeIN;
	controller2.Controller_U.InRef = df.M2_InRefIN;
	controller2.step();
	df.M2_EnabledOUT = controller2.Controller_Y.Enable;
	df.M2_StateOUT = controller2.Controller_Y.State;
	df.M2_DutyOUT = controller2.Controller_Y.DutyU;
	
	if( df.M2_StateOUT == 0 || df.M2_StateOUT == 10 ) df.M2_DutyOUT = 0; // safety kill (XXX:Fix in mat model)
	
	// limit Vref2 (if controller goes wacko), replace with 0.5 (12V at 24V PS) after testing!
	float VRef2 = df.M2_DutyOUT;
	if( VRef2 > 0.5 ) VRef2 = 0.5;
	else if( VRef2 < -0.5 ) VRef2 = -0.5;
	//-------------------------------------------------------------------------------
	//  Connect inputs of the PWM_DRV module and call the PWM signal generation macro
	// ------------------------------------------------------------------------------
    pwm1.MfuncC1 = (int16)(_IQabs(VRef2)*32768.0); // MfuncC1 is in Q15 (convert to -32768...32768 range!)
	PWM_MACRO(pwm1)		
	if(VRef2 < 0)
	{
	  Rotation2 = 0;
	}
	else
	{
	  Rotation2 = 1;
	}					
	if(Rotation2==0)
  	{
  		EPwm1Regs.AQCSFRC.bit.CSFB = 0;		//Forcing Disabled on EPWM2B	  
  		EPwm1Regs.AQCSFRC.bit.CSFA = 1;		//EPWM1A forced low
  	}
	else if(Rotation2 == 1)
  	{
  		EPwm1Regs.AQCSFRC.bit.CSFA = 0;		//Forcing Disabled on EPWM2A	  
  		EPwm1Regs.AQCSFRC.bit.CSFB = 1;		//EPWM1B forced low
  	}
	else
  	{
  		EPwm1Regs.AQCSFRC.bit.CSFA = 1;		//EPWM1A forced low
  		EPwm1Regs.AQCSFRC.bit.CSFB = 1;		//EPWM1B forced low	  
  	}	
	EPwm1Regs.CMPA.half.CMPA=pwm1.PWM1out; 
	
	// ------------------------------------------------------------------------------
	//    Connect inputs of the DATALOG module 
	// ------------------------------------------------------------------------------
    DlogCh1 = (int16)_IQtoIQ15(VRef1*32768.0);
    DlogCh2 = (int16)_IQtoIQ15(df.M1_IFdbkUIN*32768.0);
    //DlogCh3 = (int16)_IQtoIQ15(VRef2*32768.0);
    //DlogCh4 = (int16)_IQtoIQ15(IFdbk2*32768.0);

// ------------------------------------------------------------------------------
//    Call the DATALOG update function.
// ------------------------------------------------------------------------------
    dlog.update(&dlog);

	// measure INTtime
	df.INTLoopTimeUS = timerINT.ElapsedTime() * 1e6; //microseconds
	if(df.INTLoopTimeUSMAX < df.INTLoopTimeUS) df.INTLoopTimeUSMAX = df.INTLoopTimeUS;
	
// Enable more interrupts from this timer
	EPwm1Regs.ETCLR.bit.INT = 1;

// Acknowledge interrupt to recieve more interrupts from PIE group 3
	PieCtrlRegs.PIEACK.all = PIEACK_GROUP3;
}

// SETUP BOXES
void CANSetupBoxes(CANDriver* ptr)
{	
	// RX	
    ptr->SetupMBOX(0,  CAN_BASE+0x000100, 8, false); // Mode1
    ptr->SetupMBOX(1,  CAN_BASE+0x000101, 8, false); // Ref1
    ptr->SetupMBOX(2,  CAN_BASE+0x000200, 8, false); // Mode2
    ptr->SetupMBOX(3,  CAN_BASE+0x000201, 8, false); // Ref2
    
	// TX
	ptr->SetupMBOX(10,  CAN_BASE+0x000300, 8, true); // state1
	ptr->SetupMBOX(11,  CAN_BASE+0x000301, 8, true); // position1
	ptr->SetupMBOX(12,  CAN_BASE+0x000302, 8, true); // speed1
	ptr->SetupMBOX(13,  CAN_BASE+0x000303, 8, true); // current1
	ptr->SetupMBOX(14,  CAN_BASE+0x000400, 8, true); // state2
	ptr->SetupMBOX(15,  CAN_BASE+0x000401, 8, true); // position2
	ptr->SetupMBOX(16,  CAN_BASE+0x000402, 8, true); // speed2
	ptr->SetupMBOX(17,  CAN_BASE+0x000403, 8, true); // current2
	
	ptr->SetupMBOX(26,  CAN_BASE+0x000350, 8, true); // looptime
	ptr->SetupMBOX(27,  CAN_BASE+0x000351, 8, true); // totaltime
	ptr->SetupMBOX(28,  CAN_BASE+0x000352, 8, true); // looptimeMAX
	ptr->SetupMBOX(29,  CAN_BASE+0x000353, 8, true); // looptimeINT
	ptr->SetupMBOX(30,  CAN_BASE+0x000354, 8, true); // looptimeINTMAX
	
	// ctrl tuning
	ptr->SetupMBOX(20,  CAN_BASE+0x000500, 8, true); // duty
	ptr->SetupMBOX(21,  CAN_BASE+0x000501, 8, true); // current
	ptr->SetupMBOX(22,  CAN_BASE+0x000502, 8, true); // speed
	ptr->SetupMBOX(23,  CAN_BASE+0x000503, 8, true); // position
	ptr->SetupMBOX(24,  CAN_BASE+0x000504, 8, true); // ref
}
